#include "MapGen.h"
#include <malloc.h>
#include <iostream>
#include <math.h>

using namespace std;

MapGen::MapGen() {
	
	m_targetLocationCounter = 0;
	m_targetLocationSize = 5;
	m_targetLocations = new Location[m_targetLocationSize];

	// init Map.
	m_mapRows		= 200;
	m_mapColums		= 200;
	m_reductionStep	= 4;
	m_map = (unsigned char**) malloc( sizeof(unsigned char*)*m_mapRows);
	for (int i=0; i<m_mapRows; ++i) {
		m_map[i] = (unsigned char*) malloc(sizeof(unsigned char*)*m_mapColums);
		for (int y=0; y<m_mapColums; ++y) {
			m_map[i][y] = 0;
			
			// side border.
			if (y == 0 || y == m_mapColums-1) {
				m_map[i][y] = 0;
			}
			// top and bottom border.
			if (i == 0 || i == m_mapRows-1) {
				m_map[i][y] = 0;
			}
		}
	}
	
	m_sensorTypes[0].range[0] = 100;
	m_sensorTypes[0].factor[0] = 1.0;
	m_sensorTypes[0].range[1] = 150;
	m_sensorTypes[0].factor[1] = 0.8;
	m_sensorTypes[0].range[2] = 200;
	m_sensorTypes[0].factor[2] = 0.4;
	m_sensorTypes[0].range[3] = 250;
	m_sensorTypes[0].factor[3] = 0.2;
	
}

MapGen::~MapGen() {
	// free memory of map.
	for (int i=0; i<m_mapColums; ++i) {
		free(m_map[i]);
	}
	free(m_map);
	// free targets list.
	delete(m_targetLocations);
}

void MapGen::setCurrentLocation(Location inn) {	
	int *xy = LocationToSquare(inn);
	if ( (xy[0] > -1 && xy[0] < m_mapRows) &&
		(xy[1] > -1 && xy[0] < m_mapColums) ) 
	{
		int *cxy = LocationToSquare(m_currentLocation);
		if (cxy[0] != 0) {
			m_map[cxy[1]][cxy[0]] = 0;
		}
		delete cxy;
		m_map[xy[1]][xy[0]] = 255;
		m_currentLocation = inn;
	}
	delete xy;
}

Location MapGen::getAgentLocation() {
	return m_currentLocation;
}

void MapGen::setBaseLocation(Location inn) {
	int* xy = LocationToSquare(inn);
	if ( (xy[0] > 0 && xy[0] < m_mapRows) &&
		(xy[1] > 0 && xy[0] < m_mapColums) ) {
		m_map[xy[1]][xy[0]] = 254;
		m_baseLocation = inn;
	}
	delete xy;
}

int MapGen::addTargetLocaton(Location inn) {
	// Expand the array by a factor of 2 if it is full.
	int ret = -1;
	if(m_targetLocationCounter >= m_targetLocationSize-1) {
		int newSize = m_targetLocationSize*2;
		Location *tmp = new Location[newSize];
		for (int i=0; i< m_targetLocationCounter; ++i) {
			tmp[i] = m_targetLocations[i];
		}
		m_targetLocationSize = newSize;
		delete(m_targetLocations);
		m_targetLocations = tmp;
	}
	int* xy = LocationToSquare(inn);
	
	if ( (xy[0] >= 0 && xy[0] <= m_mapRows) &&
		(xy[1] >= 0 && xy[0] <= m_mapColums) ) {
		m_map[xy[1]][xy[0]] = 210+m_targetLocationCounter;
		m_targetLocations[m_targetLocationCounter] = inn;
		ret = m_targetLocationCounter;
		m_targetLocationCounter++;
	}
	delete xy;
	return ret;
}

Location MapGen::getTargetLocation(int targetNr) {
	if (targetNr > -1 && targetNr < m_targetLocationCounter) {
		return m_targetLocations[targetNr];
	}
	return Location();
}
int MapGen::getTargetLocationsCounter() {
	return m_targetLocationCounter;
}

void MapGen::changeTargetLocation(Location inn, int targetNr) {
	if (targetNr > 0 && targetNr < m_targetLocationCounter) {
		int* xy = LocationToSquare(inn);
		if ( (xy[0] > 0 && xy[0] < m_mapRows) &&
			(xy[1] > 0 && xy[0] < m_mapColums) ) {
			m_map[xy[1]][xy[0]] = 210+targetNr;
			int *cxy = LocationToSquare(m_currentLocation);
			m_map[cxy[1]][cxy[0]] = 0;
			m_targetLocations[targetNr] = inn;
		}
		delete xy;
	}
}

void MapGen::clearTargetLocations() {
	delete(m_targetLocations);
	m_targetLocations = new Location[m_targetLocationSize];
	m_targetLocationCounter = 0;
}

bool* MapGen::getBooleanMap(int threshold) {
	
	bool print = false;

	int size = m_mapColums*m_mapRows;
	bool* ret = new bool[size];
	int bmcount = 0;
	for (int y=0; y<m_mapRows; ++y) {
		for ( int x=0; x<m_mapColums; ++x) {
			if (m_map[y][x] < threshold) {
				if (print) {
					cout <<'.';
				}
				ret[bmcount] = false;
			}
			else if (m_map[y][x] == 255) {
				if (print) {
					cout <<char(169);
				}
				ret[bmcount] = false;
			}
			else if (m_map[y][x] == 254) {
				if (print) {
					cout << char(253);
				}
				ret[bmcount] = false;
			}
			else if (m_map[y][x] >200) {
				if ( print ) {
					cout << char(168);
				}
				ret[bmcount] = false;
			}
			else {
				if (print) {
					cout << '@';
				}
				ret[bmcount] = true;
			}
			bmcount++;
		}
		if (print) {
			cout << endl;
		}
	}//*/
	ret = AddBoolanMapPadding( ret, size );
	return ret;
}

bool* MapGen::AddBoolanMapPadding(bool *inn, int size) {
	//*
	bool* ret = new bool[size];
	for (int i=0; i<size; ++i) {
		ret[i] = false;
	}
	for (int i=0; i<size; ++i) {
		// Top Row.
		int lb = 1;
		int rb = 1;
		int tb = m_mapRows;
		int bb = m_mapRows;
		// top 
		if ( i<m_mapRows ) {
			tb =0;
		}
		// bottom
		else if ( i> (size-m_mapRows) ){
			bb = 0;
		}
		// left border.
		if ( i%m_mapColums == 0 ) {
			lb = 0;
		}
		// right border
		else if ( (i%m_mapColums) == m_mapColums-1 ) {
			rb = 0;
		}
		if (inn[i] == true) {
			ret[i-tb-lb]	= true; // TOP Left
			ret[i-tb]		= true;	// TOP 
			ret[i-tb+rb]	= true; // TOP RIGHT
			ret[i-lb]		= true; // Left
			ret[i]			= true; // Center
			ret[i+rb]		= true; // Right
			ret[i+bb-lb]	= true; // Below Left
			ret[i+bb]		= true; // Below
			ret[i+bb+rb]	= true; // Below Right
		}
	}
	delete inn;
	return ret;
	//*/
}
/* North is East or allong the X axis.
void MapGen::printMapOnScreen(int threshold) {
	
	cout << "MAP:"<<endl;
	cout << "Rows: "<<m_mapRows<<endl;
	cout << "Cols: "<<m_mapColums<<endl;
	cout << endl;

	for (int y=0; y<m_mapRows; ++y) {
		for ( int x=0; x<m_mapColums; ++x) {
			
			if (m_map[y][x] < threshold) {
				cout <<'.';
			}
			else if (m_map[y][x] == 255) {
				cout <<char(169);
			}
			else if (m_map[y][x] == 254) {
				cout << char(253);
			}
			else if (m_map[y][x] >200) {
				cout << char(168);
			}
			else {
				cout << '@';
			}
		}
		cout << endl;
	}
}
//*/
//* North is up, or allong the Y axis.
void MapGen::printMapOnScreen(int threshold) {
	
	cout << "MAP:"<<endl;
	cout << "Rows: "<<m_mapRows<<endl;
	cout << "Cols: "<<m_mapColums<<endl;
	cout << endl;

	for (int y=m_mapRows-1; y>0; --y) {
		for ( int x=0; x<m_mapColums; ++x) {
			
			if (m_map[x][y] < threshold) {
				cout <<'.';
			}
			else if (m_map[x][y] == 255) {
				cout <<char(169);
			}
			else if (m_map[x][y] == 254) {
				cout << char(253);
			}
			else if (m_map[x][y] >200) {
				cout << char(168);
			}
			else {
				cout << '@';
			}
		}
		cout << endl;
	}
}
//*/
int* MapGen::LocationToSquare(Location inn) {
	int *ret = new int[2];
	ret[0] = (int)inn.getX();
	ret[1] = (int)inn.getY();
	return ret;
}

void MapGen::proccessSensorReading(double range, double bearing, int sensorType) {
	if (m_sensorTypes[sensorType].range[3] < range) {
		return;
	}
	
	for( int x = 0; x < m_mapColums; ++x ) {
		for ( int y = 0; y < m_mapRows; ++y) {
			m_map[x][y] -= m_reductionStep;
		}
	}

	if (bearing >= 360) {
		bearing = (int)bearing % 360;
	}
	double confidence = 0.0;
	double incremetLevel = 25;

	for (int i=0; i<4; ++i) {
		if (m_sensorTypes[sensorType].range[i] > range) {
			confidence = m_sensorTypes[sensorType].factor[i];
			break;
		}
	}
	long double Per = 3.1415926535897932384626433832795;

	bearing = (Per/180)*bearing;
	double b = (Per/180)*m_currentLocation.getB();
	double x = m_currentLocation.getX()*10;
	double y = m_currentLocation.getY()*10;
	double newX = 0;
	double newY = 0;
	
	// RtX =	[cos(t)	-sin(t)] * [x]
	//			[sin(t)  cos(t)]   [y]
	// newX = cos(t)*x + -sin(t)*y;
	// newY = sin(t)*x + cos(t)*y;
	newX = cos(bearing)*range + (-sin(bearing)*newY);
	newY = sin(bearing)*range + (sin(bearing)*newY);

	newX += x;
	newY += y;
	
	int gx = newX /10;
	int gy = newY /10;
	if ( gx < 0 || gx > m_mapColums) {
		// x is out of bounds.
	}
	else if (gy < 0 || gy > m_mapRows) {
		// y is out of bounds.
	}
	else {
		if (m_map[gy][gx] < 200*confidence) {
			m_map[gy][gx] += incremetLevel*confidence;
		}
	}
}